module avd

    use, intrinsic :: iso_c_binding
    use basic
    use poly
    implicit none

contains

subroutine acc2vd(a,v,d,n,dt,v0,d0) bind(c)
!     加速度序列积分为速度、位移

    real(8), intent(in) :: dt,v0,d0
    integer(4), intent(in) :: n
    real(8), intent(in) :: a(n)
    real(8), intent(out) :: v(n), d(n)

    integer(4) :: i
    real(8) :: vi, di

    v(1) = v0
    d(1) = d0

    vi = 0.D0
    di = 0.D0
    do i = 2, n, 1
        vi = vi + dt*(a(i)+a(i-1))*0.5
        v(i) = v0 + vi
        di = di + dt*(v(i)+v(i-1))*0.5
        d(i) = d0 + di
    end do
    return

end subroutine acc2vd

subroutine ratacc2vd(a,v,d,n,dt,v0,d0) bind(c)
!     估计初始速度和位移后再积分得到速度和位移

    real(8), intent(in) :: dt,v0,d0
    integer(4), intent(in) :: n
    real(8), intent(in) :: a(n)
    real(8), intent(out) :: v(n), d(n)

    real(8) :: vn, dn
    integer :: i

    call acc2vd(a,v,d,n,dt,v0,d0)
    vn = sum(v)/dble(n)
    dn = 0.0d0
    do i = 1, n, 1
        dn = dn + d(i) - vn*dble(i-1)*dt
    end do
    dn = dn/dble(n)

    call acc2vd(a,v,d,n,dt,v0-vn,d0-dn)

end subroutine ratacc2vd

subroutine targetdc(a,td,n,tp,ntp,ph,pl,dt,v0,d0) bind(c)
!     根据给定目标位移，调整加速度时程
    integer(4), intent(in) :: n
    real(8), intent(in) :: dt,v0,d0
    real(8), intent(in) :: td(n)
    real(8), intent(out) :: a(n)
    integer(4), intent(in) :: ntp,ph,pl
    integer(4), intent(in) :: tp(ntp)

    integer :: i,j,k,pn
    real(8), allocatable :: M(:,:), b(:), tc(:)
    real(8), allocatable :: v(:), d(:), t(:)

    pn = ph - pl + 1
    if ( pn>ntp ) pn = ntp

    allocate(M(ntp,pn))
    allocate(b(ntp))
    allocate(tc(ntp))
    allocate(t(n))
    allocate(v(n))
    allocate(d(n))

    t = [ (((i-1)*dt),i=1,n) ]
    call acc2vd(a,v,d,n,dt,v0,d0)

    do k = 1, ntp, 1
        j = tp(k)
        tc(k) = t(j)
        b(k)  = td(j)-d(j)
        do i = pl, pl+pn-1, 1
            M(k,i-pl+1) = dpower(tc(k),i,0)
        end do
    end do

    call leastsqs(M,b,ntp,pn)

    do k = 1, n, 1
        do i = pl, pl+pn-1, 1
            a(k) = a(k)+b(i-pl+1)*dpower(t(k),i,2)
        end do
    end do

    deallocate(M)
    deallocate(b)
    deallocate(tc)
    deallocate(t)
    deallocate(v)
    deallocate(d)

    return

end subroutine targetdc

subroutine targetdvc(a,td,tv,n,tp,ntp,ph,pl,dt,v0,d0) bind(c)
!     根据给定目标位移、速度，调整加速度时程
    integer(4), intent(in) :: n
    real(8), intent(in) :: dt,v0,d0
    real(8), intent(in) :: td(n),tv(n)
    real(8), intent(out) :: a(n)
    integer(4), intent(in) :: ntp,ph,pl
    integer(4), intent(in) :: tp(ntp)

    integer :: i,j,k,pn
    real(8), allocatable :: M(:,:), b(:), tc(:)
    real(8), allocatable :: v(:), d(:), t(:)

    pn = (ph - pl + 1)*2
    if ( pn>2*ntp ) pn = 2*ntp

    allocate(M(2*ntp,pn))
    allocate(b(2*ntp))
    allocate(tc(ntp))
    allocate(t(n))
    allocate(v(n))
    allocate(d(n))

    t = [ (((i-1)*dt),i=1,n) ]
    call acc2vd(a,v,d,n,dt,v0,d0)

    do k = 1, ntp, 1
        j = tp(k)
        tc(k) = t(j)
        b(k)  = td(j)-d(j)
        b(k+ntp)  = tv(j)-v(j)
        do i = pl, pl+pn-1, 1
            M(k,i-pl+1) = dpower(tc(k),i,0)
            M(k+ntp,i-pl+1) = dpower(tc(k),i,1)
        end do
    end do

    call leastsqs(M,b,2*ntp,pn)

    do k = 1, n, 1
        do i = pl, pl+pn-1, 1
            a(k) = a(k)+b(i-pl+1)*dpower(t(k),i,2)
        end do
    end do

    deallocate(M)
    deallocate(b)
    deallocate(tc)
    deallocate(t)
    deallocate(v)
    deallocate(d)

    return

end subroutine targetdvc

subroutine targetdvac(a,td,tv,ta,n,tp,ntp,ph,pl,dt,v0,d0) bind(c)
!     根据给定目标位移、速度、加速度，调整加速度时程
    integer(4), intent(in) :: n
    real(8), intent(in) :: dt,v0,d0
    real(8), intent(in) :: td(n),tv(n),ta(n)
    real(8), intent(out) :: a(n)
    integer(4), intent(in) :: ntp,ph,pl
    integer(4), intent(in) :: tp(ntp)

    integer :: i,j,k,pn
    real(8), allocatable :: M(:,:), b(:), tc(:)
    real(8), allocatable :: v(:), d(:), t(:)

    pn = (ph - pl + 1)*3
    if ( pn>3*ntp ) pn = 3*ntp

    allocate(M(3*ntp,pn))
    allocate(b(3*ntp))
    allocate(tc(ntp))
    allocate(t(n))
    allocate(v(n))
    allocate(d(n))

    t = [ (((i-1)*dt),i=1,n) ]
    call acc2vd(a,v,d,n,dt,v0,d0)

    do k = 1, ntp, 1
        j = tp(k)
        tc(k) = t(j)
        b(k)  = td(j)-d(j)
        b(k+ntp)  = tv(j)-v(j)
        b(k+2*ntp)  = ta(j)-a(j)
        do i = pl, pl+pn-1, 1
            M(k,i-pl+1) = dpower(tc(k),i,0)
            M(k+ntp,i-pl+1) = dpower(tc(k),i,1)
            M(k+2*ntp,i-pl+1) = dpower(tc(k),i,2)
        end do
    end do

    call leastsqs(M,b,3*ntp,pn)

    do k = 1, n, 1
        do i = pl, pl+pn-1, 1
            a(k) = a(k)+b(i-pl+1)*dpower(t(k),i,2)
        end do
    end do

    deallocate(M)
    deallocate(b)
    deallocate(tc)
    deallocate(t)
    deallocate(v)
    deallocate(d)

    return

end subroutine targetdvac

subroutine polyblc(a,n,oh,ol,dt,v0,d0) bind(c)
    
    integer, intent(in) :: n
    real(8), intent(in) :: dt, v0, d0
    real(8), intent(inout) :: a(n)
    integer, intent(in) :: oh, ol

    integer :: i,l
    real(8), allocatable :: t(:), v(:), d(:), c(:), r(:), ac(:)

    allocate(t(n))
    allocate(v(n))
    allocate(d(n))
    allocate(ac(n))

    allocate(c(oh+2))
    l = oh+1
    allocate(r(l))

    t = [ (((i-1)*dt),i=1,n) ]

    call cumtrapz(a,v,n,dt,v0)
    call polyfit(v,t,n,c,oh+1,ol+1)
    call polyder(c,oh+2,1,r,l)
    call polyval(r,oh+1,t,ac,n)
    a = a - ac

    deallocate(c)
    allocate(c(oh+3))

    call acc2vd(a,v,d,n,dt,v0,d0)
    call polyfit(d,t,n,c,oh+2,ol+2)
    call polyder(c,oh+3,2,r,l)
    call polyval(r,oh+1,t,ac,n)

    a = a - ac

    deallocate(t)
    deallocate(ac)
    deallocate(v)
    deallocate(d)
    deallocate(c)
    deallocate(r)

    return
end subroutine polyblc

end module avd